/*********************************************************
 *
 * Filename: cm_camera.c
 *   Author: jmdvirus
 *   Create: 2020年03月17日 星期二 12时27分28秒
 *
 *********************************************************/

#include <stdio.h>

#include "cm_camera.h"

char *get_format_name(unsigned int format)
{
    if(format == V4L2_PIX_FMT_YUV422P)
        return "YUV422P";
    else if(format == V4L2_PIX_FMT_YUV420)
       return "YUV420";
    else if(format == V4L2_PIX_FMT_YVU420)
       return "YVU420";
    else if(format == V4L2_PIX_FMT_NV16)
        return "NV16";
    else if(format == V4L2_PIX_FMT_NV12)
        return "NV12";
    else if(format == V4L2_PIX_FMT_NV61)
        return "NV61";
    else if(format == V4L2_PIX_FMT_NV21)
        return "NV21";
    else if(format == V4L2_PIX_FMT_HM12)
       return "MB YUV420";
    else if(format == V4L2_PIX_FMT_YUYV)
       return "YUYV";
    else if(format == V4L2_PIX_FMT_YVYU)
       return "YVYU";
    else if(format == V4L2_PIX_FMT_UYVY)
       return "UYVY";
    else if(format == V4L2_PIX_FMT_VYUY)
        return "VYUY";
    else if(format == V4L2_PIX_FMT_MJPEG)
        return "MJPEG";
    else if(format == V4L2_PIX_FMT_H264)
        return "H264";
    else
    return NULL;
}

#ifdef __USE_VIN_ISP__
static int getSensorType(int fd)
{
    int ret = -1;
    struct v4l2_control ctrl;
    struct v4l2_queryctrl qc_ctrl;

    if (fd == 0)
        return 0xFF000000;

    memset(&ctrl, 0, sizeof(struct v4l2_control));
    memset(&qc_ctrl, 0, sizeof(struct v4l2_queryctrl));
    ctrl.id = V4L2_CID_SENSOR_TYPE;
    qc_ctrl.id = V4L2_CID_SENSOR_TYPE;

    if (-1 == ioctl (fd, VIDIOC_QUERYCTRL, &qc_ctrl)){
        printf(" query sensor type ctrl failed\n");
        return -1;
    }

    ret = ioctl(fd, VIDIOC_G_CTRL, &ctrl);
    if(ret < 0){
        printf(" get sensor type failed,errno(%d)\n", errno);
        return ret;
    }

    return ctrl.value;
}
#endif

int cm_camera_init(camera_handle *h, int index, int width, int height)
{
	if (!h) return -1;
	memset(h, 0, sizeof(camera_handle));

	h->camera_index = index;
	h->win_width = width;
	h->win_height = height;

	return 0;
}

int cm_camera_deinit(camera_handle *h)
{
	if (!h) return -1;
	return 0;
}

int cm_camera_open(camera_handle *h)
{
	if (!h) return -1;
	
    struct v4l2_capability cap;      /* Query device capabilities */
    struct v4l2_fmtdesc fmtdesc;     /* Enumerate image formats */
    struct v4l2_frmsizeenum frmsize; /* Enumerate frame sizes */
    struct v4l2_format fmt;          /* try a format */
    struct v4l2_input inp;           /* select the current video input */
    struct v4l2_streamparm parms;    /* set streaming parameters */
    struct v4l2_requestbuffers req;  /* Initiate Memory Mapping or User Pointer I/O */
    struct v4l2_buffer buf;          /* Query the status of a buffer */

    int n_buffers = 0;

	char camera_path[32] = {0};
	sprintf(camera_path, "%s%d", "/dev/video", h->camera_index);
    h->videofd = open(camera_path, O_RDWR | O_NONBLOCK, 0);
	if (h->videofd < 0) {
		return -2;
	}

	memset(&cap, 0, sizeof(cap));
	int ret = ioctl(h->videofd, VIDIOC_QUERYCAP, &cap);
	if (ret < 0) {
		printf("Error: querycap\n");
	} else {
	}

    if(strcmp((char*)cap.driver,"sunxi-vfe") == 0){
        h->camera_type = vfe_camera;
    }else if(strcmp((char*)cap.driver,"sunxi-vin") == 0){
        h->camera_type = vin_camera;
    }else if(strcmp((char*)cap.driver,"uvcvideo") == 0){
        h->camera_type = usb_camera;
    }else{
        printf(" unknow camera type is %s\n",cap.driver);
        close(h->videofd);
        return -3;
    }

    if((cap.capabilities & (V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_CAPTURE_MPLANE)) <= 0){
        printf(" The device is not supports the Video Capture interface!!!\n");
        close(h->videofd);
        return -4;
    }

    inp.index = 0;
    inp.type = V4L2_INPUT_TYPE_CAMERA;
    if(ioctl(h->videofd,VIDIOC_S_INPUT,&inp) < 0){
        printf(" VIDIOC_S_INPUT failed! s_input: %d\n",inp.index);
        close(h->videofd);
        return -5;
    }

#ifdef __USE_VIN_ISP__
    /* detect sensor type */
    h->sensor_type = getSensorType(h->videofd);
    if(h->sensor_type == V4L2_SENSOR_TYPE_RAW){
        h->ispPort = CreateAWIspApi();
        printf(" raw sensor use vin isp\n");
    }
#endif

    memset(&fmt,0,sizeof(fmt));
    if(h->camera_type == vin_camera){
        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        fmt.fmt.pix_mp.width = h->win_width;
        fmt.fmt.pix_mp.height = h->win_height;
        fmt.fmt.pix_mp.field = V4L2_FIELD_NONE;
    }else{
        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        fmt.fmt.pix.width = h->win_width;
        fmt.fmt.pix.height = h->win_height;
        fmt.fmt.pix.field = V4L2_FIELD_NONE;
    }

    h->pixelformat = V4L2_PIX_FMT_YUV420;
	fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_YUV420;

	ret = ioctl(h->videofd, VIDIOC_S_FMT, &fmt);
	if (ret < 0) {
		printf("Error control fmt\n");
		close(h->videofd);
		return -6;
	}

    if(h->camera_type == vin_camera)
        parms.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    else
        parms.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    parms.parm.capture.timeperframe.numerator = 1;
    parms.parm.capture.timeperframe.denominator = 30;
    ret = ioctl(h->videofd,VIDIOC_S_PARM,&parms);
	if (ret < 0) {
		printf("Error control params\n");
		close(h->videofd);
		return -7;
	}

    if(h->camera_type == vin_camera){
        ret = ioctl(h->videofd, VIDIOC_G_FMT, &fmt);
		if (ret >= 0) {
            h->nplanes = fmt.fmt.pix_mp.num_planes;
		}
        printf(" VIDIOC_S_FMT succeed\n");
        printf(" fmt.type = %d\n",fmt.type);
        printf(" fmt.fmt.pix.width = %d\n",fmt.fmt.pix_mp.width);
        printf(" fmt.fmt.pix.height = %d\n",fmt.fmt.pix_mp.height);
        printf(" fmt.fmt.pix.pixelformat = %s\n",get_format_name(fmt.fmt.pix_mp.pixelformat));
        printf(" fmt.fmt.pix.field = %d\n",fmt.fmt.pix_mp.field);
        printf("nplanes:%d\n", h->nplanes);
	}

    memset(&req, 0, sizeof(struct v4l2_requestbuffers));
    req.count = 3;
    if(h->camera_type == vin_camera)
        req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    else
        req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;
    if(ioctl(h->videofd, VIDIOC_REQBUFS, &req) < 0){
		printf(" VIDIOC_REQBUFS failed\n");
        close(h->videofd);
        return -8;
    }

    h->buf_count = req.count;
    printf(" reqbuf number is %d\n",h->buf_count);

    h->buffers = (struct buffer*)calloc(req.count, sizeof(struct buffer));
    for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
        memset(&buf, 0, sizeof(struct v4l2_buffer));
        if(h->camera_type == vin_camera)
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        else
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = n_buffers;
        if(h->camera_type == vin_camera){
            printf( "length:%d : nplanes:%d\n", buf.length, h->nplanes );
            buf.length = h->nplanes;
            buf.m.planes =  (struct v4l2_plane *)calloc(buf.length, sizeof(struct v4l2_plane));
        }

        if (ioctl(h->videofd, VIDIOC_QUERYBUF, &buf) == -1) {
            printf(" VIDIOC_QUERYBUF error\n");

            if(h->camera_type == vin_camera)
                free(buf.m.planes);
            free(h->buffers);

            close(h->videofd);

            return -9;
        }

        if(h->camera_type == vin_camera){
            for(int i = 0; i < h->nplanes; i++){
                h->buffers[n_buffers].length[i] = buf.m.planes[i].length;
                h->buffers[n_buffers].start[i] = mmap(NULL , buf.m.planes[i].length,
                                                   PROT_READ | PROT_WRITE, \
                                                   MAP_SHARED , h->videofd, \
                                                   buf.m.planes[i].m.mem_offset);

                printf(" map buffer index: %d, mem: %p, len: %x, offset: %x\n",
                   n_buffers, h->buffers[n_buffers].start[i],buf.m.planes[i].length,
                   buf.m.planes[i].m.mem_offset);
            }
            free(buf.m.planes);
        }else{
            h->buffers[n_buffers].length[0] = buf.length;
            h->buffers[n_buffers].start[0] = mmap(NULL , buf.length,
                                                PROT_READ | PROT_WRITE, \
                                                MAP_SHARED , h->videofd, \
                                                buf.m.offset);
			printf(" map buffer index: %d, mem: %p, len: %x, offset: %x\n", \
                    n_buffers, h->buffers[n_buffers].start[0],buf.length,buf.m.offset);
        }
    }

    for(n_buffers = 0; n_buffers < req.count; n_buffers++) {
        memset(&buf, 0, sizeof(struct v4l2_buffer));
        if(h->camera_type == vin_camera)
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
        else
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory= V4L2_MEMORY_MMAP;
        buf.index= n_buffers;
        if(h->camera_type == vin_camera){
            buf.length = h->nplanes;
            buf.m.planes =  (struct v4l2_plane *)calloc(buf.length, sizeof(struct v4l2_plane));
        }

        if (ioctl(h->videofd, VIDIOC_QBUF, &buf) == -1) {
            printf(" VIDIOC_QBUF error\n");

            if(h->camera_type == vin_camera)
                free(buf.m.planes);
            free(h->buffers);

            close(h->videofd);
            return -10;
        }
        if(h->camera_type == vin_camera)
            free(buf.m.planes);
    }
	return 0;
}

int cm_camera_close(camera_handle *h) {
	if (!h) return -1;

    if(h->camera_type == vin_camera){
        for (int i = 0; i < h->buf_count; ++i)
            for (int j = 0; j < h->nplanes; j++)
                 munmap(h->buffers[i].start[j], h->buffers[i].length[j]);
    }else{
        for(int i=0; i<h->buf_count; i++)
            munmap(h->buffers[i].start[0],h->buffers[i].length[0]);
    }
	if (h->videofd > 0) {
		close(h->videofd);
	}
	h->videofd = 0;
	if (h->buffers) {
		free(h->buffers);
	}

	h->buffers = NULL;
	return 0;
}

int cm_camera_start(camera_handle *h)
{
	if (!h || h->videofd <= 0) return -1;

	int type = 0;
    if(h->camera_type == vin_camera)
        type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    else
        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if(ioctl(h->videofd, VIDIOC_STREAMON, &type) == -1) {
        printf(" VIDIOC_STREAMON error! %s\n",strerror(errno));
		return -2;
    }

#ifdef __USE_VIN_ISP__
    /* setting ISP */
    if(h->sensor_type == V4L2_SENSOR_TYPE_RAW){
        h->ispId = -1;
        h->ispId = h->ispPort->ispGetIspId(0);
        if(h->ispId >= 0)
            h->ispPort->ispStart(h->ispId);
    }
#endif

    memset(&h->buf, 0, sizeof(struct v4l2_buffer));
    if(h->camera_type == vin_camera)
        h->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    else
        h->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    h->buf.memory = V4L2_MEMORY_MMAP;
    if(h->camera_type == vin_camera){
        h->buf.length = h->nplanes;
        h->buf.m.planes = (struct v4l2_plane *)calloc(h->nplanes, sizeof(struct v4l2_plane));
    }

	return 0;
}

int cm_camera_stop(camera_handle *h) 
{
	if (!h || h->videofd <= 0) return -1;

    if(h->camera_type == vin_camera)
        free(h->buf.m.planes);
#ifdef __USE_VIN_ISP__
    /* stop ISP */
    if(h->sensor_type == V4L2_SENSOR_TYPE_RAW
                                    && h->ispId >= 0){
        h->ispPort->ispStop(h->ispId);
        DestroyAWIspApi(h->ispPort);
        h->ispPort = NULL;
    }
#endif
    int type = 0;
    if(h->camera_type == vin_camera)
        type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    else
        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if(ioctl(h->videofd, VIDIOC_STREAMOFF, &type) == -1)
        printf(" VIDIOC_STREAMOFF error! %s\n",strerror(errno));

	return 0;
}

int cm_camera_stream_get(camera_handle *h, unsigned char *data, int len)
{
	if (!h || h->videofd <= 0) return -1;
	
    fd_set fds;
    FD_ZERO(&fds);
    FD_SET(h->videofd, &fds);

    struct timeval tv;
    tv.tv_sec = 10;
    tv.tv_usec = 0;

    int ret = select(h->videofd+1,&fds,NULL,NULL,&tv);
	if (ret == -1) {
		return EAGAIN;
	} else if (ret == 0) {
		return -2;
	}
    ret = ioctl(h->videofd, VIDIOC_DQBUF, &h->buf);
	if (ret != 0) {
		return -3;
	}

	// data save
	memcpy(data, h->buffers[h->buf.index].start[0], h->buffers[h->buf.index].length[0]);

    if (ioctl(h->videofd, VIDIOC_QBUF, &h->buf) != 0)
	{
		printf("qbuf error\n");
	}

	return 0;
}

int cm_camera_stream_get2(camera_handle *h, unsigned char **data, size_t* len)
{
	if (!h || h->videofd <= 0) return -1;

    fd_set fds;
    FD_ZERO(&fds);
    FD_SET(h->videofd, &fds);

    struct timeval tv;
    tv.tv_sec = 10;
    tv.tv_usec = 0;

    int ret = select(h->videofd+1,&fds,NULL,NULL,&tv);
	if (ret == -1) {
		return EAGAIN;
	} else if (ret == 0) {
		return -2;
	}
    ret = ioctl(h->videofd, VIDIOC_DQBUF, &h->buf);
	if (ret != 0) {
		return -3;
	}

	// data save
//	memcpy(data, h->buffers[h->buf.index].start[0], h->buffers[h->buf.index].length[0]);
    if (len)
    {
        *len = h->buffers[h->buf.index].length[0];
    }
    if (data)
    {
        *data = (unsigned char*)(h->buffers[h->buf.index].start[0]);
    }
    if (ioctl(h->videofd, VIDIOC_QBUF, &h->buf) != 0)
    {
        printf("qbuf error\n");
    }
	return len != NULL ? *len : 1;
}

int cm_camera_stream_qbuf(camera_handle *h)
{
    if (!h || h->videofd <= 0) return -1;

    if (ioctl(h->videofd, VIDIOC_QBUF, &h->buf) != 0)
    {
        printf("qbuf error\n");
    }
    return 0;
}
